%% UKF估计非线性离散系统
clear all;
clc;

%% 参数初始化
N = 200;
T = 0.05;
t = T:T:N*T;
X0 = [1; 0];
P0 = [1, 0; 0, 1];
mu = [0, 0];
matQ = [0.01, 0; 0, 0.0001];
matR = [0.1, 0; 0, 0.1];

%% 生成真值和测量值
rng(1);
Wk = mvnrnd(mu, matQ, N)';
Vk = mvnrnd(mu, matR, N)';
realXk = zeros(2, N);
measureXk = zeros(2, N);

realXk(:, 1) = X0;
measureXk(1, 1) = 2*sin(realXk(1,1)/2) + Vk(1,1);
measureXk(2, 1) = realXk(1,1)/2 + Vk(2,1);
for i = 2:1:N
    x1pre = realXk(1, i-1);
    x2pre = realXk(2, i-1);
    realXk(1, i) = x1pre + T*x2pre + Wk(1, i-1);
    realXk(2, i) = -10*T*sin(x1pre) + (1-T)*x2pre + Wk(2, i-1);
    measureXk(1, i) = 2*sin(realXk(1,i)/2) + Vk(1,i);
    measureXk(2, i) = realXk(1,i)/2 + Vk(2,i);
end

%% UKF
alpha = 0.1;
beta = 2;
kappa = 1;
L = 2; % 状态维数
lamda = alpha^2*(L+kappa)-L;

%% 5_4使用matlab自带的UKF
% 初值
estimateXpre = X0;
estimateX_UKF_M = zeros(2, N);
estimateX_UKF_M(:, 1) = estimateXpre;

obj = unscentedKalmanFilter(@StateFcn,@MeasurementFcn,estimateX_UKF_M(:,1),...
        'Alpha', 0.1, ...
        'Beta', 2, ...
        'Kappa', 1, ...
        'ProcessNoise', matQ, ...
        'MeasurementNoise', matR);

for i = 1:1:N
    % 先correct后prdecit, 记录i而不是i+1
    [CorrectedState, CorrectedStateCovariance] = correct(obj, measureXk(:,i));
    [PredictedState, PredictedStateCovariance] = predict(obj);
    estimateX_UKF_M(:,i) = CorrectedState;
end

%% 画图
% 真值vs自写UKFvs官方UKF
figure;
plot(t, realXk, 'LineWidth', 1.5);
hold on;
plot(t, estimateX_UKF_M, 'LineWidth', 1.5);
legend('realx1', 'realx2', 'UKFx1_M', 'UKFx2_M');
title('真值vs官方UKF');

%% 模型函数
function [ x ] = StateFcn( x )
    T = 0.05;
    x = [x(1)+T*x(2); -10*T*sin(x(1))+(1-T)*x(2)];
end

function [ z ] = MeasurementFcn( x )
    z = [2*sin(x(1)/2); x(1)/2];
end
